/*
 * AS6500_data_driver.c
 *
 *  Created on: 2021年10月24日
 *      Author: zwr
 */

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "LPC8xx.h"
#include "spi.h"
#include "syscon.h"
#include "swm.h"
#include "utilities.h"
#include "gpio.h"
#include "uart.h"
#include "chip_setup.h"

extern unsigned char MySPIByte(unsigned char sdat,unsigned char cs,unsigned char rxignore);
extern void delay1ms(void);         // delay 86*13*12/30 = 0.472ms
extern void DAC1220_writeV(double v);
extern int sign(double x);
extern double round(double r);
extern void delay1ms(void);  // delay 86*13*12/30 = 0.472ms
//控制器参数
extern double eita;
extern double miu;
extern double rou;
extern double lamda;
extern double epsilon;
extern int print_flag;
//控制器伪偏导数初值
extern double fai1,fai2;
extern double error2;
 extern double td;     //期望值  --->    40.000 006 852 +6
 extern double yd; //PPB
 extern double y1,y2,y3;
 extern double dac_data1,dac_data2;//类型可能要改 unsigned int
 extern double tdc_data;
 extern double du1,du2;//电压数据差
 extern double sum;
 extern double TT;
/*********************************/
 extern const char config_register[17];
 /* CFG0: 0x83 PIN_ENA_RSTIDX |  PIN_ENA_STOP1  |PIN_ENA_STOP2
  * CFG1: 0x03  HIT_ENA_STOP1 |  HIT_ENA_STOP2
  * CFG2: 0x00  CFG2_FIXED_VALUE
  * CFG3: 0x40     //0xF1, 0xFA, 0x02, /// REFCLK_DIVISIONS is 0x2FAF1  195313 in case of 5.120MHz quartz crystal and 1ps resolution
  * CFG4: 0x0D
  * CFG5: 0x03    //0x030D40 = 20 000  REFCLK_DIVISIONS is 0x030D40  200 000 in case of 5MHz quartz crystal and 1ps resolution
  * CFG6: 0xC0  CFG6_FIXED_VALUE
  * CFG7: 0XA3  CFG7_FIXED_VALUE | REFCLK_BY_XOSC  0x23 | 0x80
  * CFG8: 0xA1  CFG8_FIXED_VALUE
  * CFG9: 0x13  CFG9_FIXED_VALUE
  * CFG10: 0x00 CFG10_FIXED_VALUE
  * CFG11: 0x0A CFG11_FIXED_VALUE
  * CFG12: 0xCC CFG12_FIXED_VALUE
  * ,0x05,0xF1,0x7D,0x04 ... FIXED_VALUE
  * REF_CLK = 2 * 10^5 ps  resolution = 1 ps
   * */

 extern int irq_flag;
 extern double Vcc;
 uint32_t index1_old = 0;
 uint32_t result1_old = 0;

/* as6500 configure */
double tdc6500()
{
	char rx_config_register[17] = {0};
	unsigned char buffer=0;
	uint32_t index[4]={0,0};
	uint32_t  result[4]={0,0};
	double T=0;

	// dac1220_set(rx_buffer[0],rx_buffer[1],rx_buffer[2]);

	MySPIByte(0x30,1,1);//Power_On_Reset
	delay1ms();

	// Writing to the configuration registers (CR)
	MySPIByte(0x80,0,1);//Write Configuration   opcode for "Write Configuration"
	for (int i = 0; i < 16; i++) // Send all 17 config registers via SPI
	{
		MySPIByte( config_register[i],0,1);
	}
	MySPIByte( config_register[16],1,1);
	delay1ms();//

	MySPIByte(0x40,0,1);//read Configuration  // Opcode for "Read Configuration" and config address (00) are sent over SPI
	for (int i = 0; i < 16; i++) // read all 17 config registers via SPI
	{
		rx_config_register[i] = MySPIByte(0x00,0,0);
	}
	rx_config_register[16] = MySPIByte(0x00,1,0);
	delay1ms();
	//printf("rx_config_register: ");
	//for(int ii = 0; ii < 17; ii++){
	//	printf("%u  ",rx_config_register[ii]);
	//}
	//printf("\n");
    //rst_id
	GPIOSetBitValue( 1, 17, 1);
	delay1ms();
	delay1ms();
	GPIOSetBitValue( 1, 17, 0);

	MySPIByte(0x18,1,1);//Init  // opcode for "Initialize Chip and Start
	delay1ms();//no must
	delay1ms();//no must

    //begin test
	GPIOSetBitValue( 1, 0, 1); //FPGA begin work
	delay1ms();//no must
	delay1ms();//no must
	delay1ms();//no must
	delay1ms();//no must
	delay1ms();//no must
	delay1ms();//no must
	while(GPIOGetPinValue(0, 0)); // wait as6500 ready interrupt

	MySPIByte(0x60+8,0,1);  //spiopc_read_results + reference_index_ch1_byte3  // Opcode for "Read Result" and data address are sent
	delay1ms();//no must
	delay1ms();//no must
	for(int i=0;i<100;i++)
	{delay1ms();}//调整


	for(int i=0;i<4;i++){
	  // The complete reference index (3 Bytes) has been received.
	  buffer = 0;
	  buffer=MySPIByte(0x00,0,0);
	  index[i]=index[i]+(buffer<<16);

	  buffer=MySPIByte(0x00,0,0);
	  index[i]=index[i]+(buffer<<8);

	  buffer=MySPIByte(0x00,0,0);
	  index[i]=index[i]+buffer;

	  // The complete stop result (3 Bytes) has been received
	  buffer=MySPIByte(0x00,0,0);
	  result[i]=result[i]+(buffer<<16);

	  buffer=MySPIByte(0x00,0,0);
	  result[i]=result[i]+(buffer<<8);

	  buffer=MySPIByte(0x00,0,0);
	  result[i]=result[i]+buffer;

	}
	if(print_flag == 1){
		//printf("\r REFID0 = %u   REFID1 = %u  RESULT0 = %u  RESULT1 = %u     REFID1 - REFID0 = %u  RESULT1 - RESULT0 = %u \n\r",index[0],index[1],result[0],result[1],index[1]- index[0],result[1]-result[0]);
		print_flag = 0;
	}//index[0] = 0,index[1] = 0,buffer = 0;


	/*
	result1_old =  result[0];
	index1_old  =  index[0];
	while(GPIOGetPinValue(0, 0)); // wait as6500 ready interrupt
	MySPIByte(0x60+8,0,1);  //spiopc_read_results + reference_index_ch1_byte3  // Opcode for "Read Result" and data address are sent
	for(int i=0;i<2;i++){
	  // The complete reference index (3 Bytes) has been received.
	  buffer=MySPIByte(0x00,0,0);
	  index[i]=index[i]+(buffer<<16);

	  buffer=MySPIByte(0x00,0,0);
	  index[i]=index[i]+(buffer<<8);

	  buffer=MySPIByte(0x00,0,0);
	  index[i]=index[i]+buffer;

	  // The complete stop result (3 Bytes) has been received
	  buffer=MySPIByte(0x00,0,0);
	  result[i]=result[i]+(buffer<<16);

	  buffer=MySPIByte(0x00,0,0);
	  result[i]=result[i]+(buffer<<8);

	  buffer=MySPIByte(0x00,0,0);
	  result[i]=result[i]+buffer;

	}
	*/

	// T=((float)result[1]-(float)result[0])/1.0e6+((float)index[1]-(float)index[0])*0.200000;
	//T=((float)result[1]-(float)result[0])/1.0e6+((float)index[1]-(float)index[0])/5;
	T=((double)result[1]-(double)result[0])/1.0e6+((double)index[1]-(double)index[0])/5;//修改了类型 us + us
	TT = T;
	//double stop1_T =((double)index[0]-(double)index1_old)/5;

	if(print_flag == 1){
	//printf("*******************************************************\n");
	//printf("\r current frequency : %10.6f  REFID1-REFID0 = %u  index0 = %u \n\r", T,index[1]-index[0],index[0]);
	}
	//printf("\r   REFID1-REFID0 = %u  \n\r",index[0]-index1_old);

	//irq_flag=0;
    GPIOSetBitValue( 1, 0, 0);//fpga down
    return T;
}

void data_driver()
{
	unsigned char buf_hex[3]={0};
	fai2=fai1+eita*du1*(y2-y1-fai1*du1)/(miu+du1*du1);
	//fai0=fai1;
	if ((abs(fai2)<epsilon) || (du1<(epsilon/10000)) || (sign(fai2) != sign(0.3)))
        {
		  fai2=sign(0.3)*0.01;
		}
		dac_data2 = dac_data1+rou*fai2*(yd-y2)/(lamda+fai2*fai2);
    	dac_data2 = round(dac_data2); //大于x 的最小整数
    	if ( dac_data2>16777215 )
          dac_data2 = 16777215;
        if ( dac_data2<0 )
           dac_data2 = 0;


        print_flag = 1;
		for(int i=0;i<10;i++)
		{
		//DAC传值
       	buf_hex[0]=(unsigned int)(dac_data2*16)&0xff;
       	buf_hex[1]=((unsigned int)(dac_data2*16)>>8)&0xff;
       	buf_hex[2]=((unsigned int)(dac_data2*16)>>16)&0xff;
       	dac1220_set(buf_hex[2],buf_hex[1],buf_hex[0]);  //annotation by zwr
       	//读取TDC数据
		tdc_data=tdc6500();
		sum+=tdc_data;
		}
		/* dac write VCC */
		//DAC1220_writeV(Vcc);

		sum=sum/10;
		printf("2. *************************************\n");
		printf("\r2.dac_data2: %10.6f\n\r",dac_data2);
		printf("\r2. As6500 measure period : %10.6f us\n\r", sum);
		error2=sum-td;
		y3=-error2/(td-error2)*1e10;//  e9????
		du2=dac_data2-dac_data1;

		//迭代赋值
		 fai1=fai2;
		 du1=du2;
		 y1=y2;
		 y2=y3;
		 dac_data1=dac_data2;
		 sum=0;
}
